
from numpy import zeros
import vtk

class Pose(vtk.vtkTransform):
    """
    This class will store the pose information for models or volumes that are
    being registered.

    It is subclassed from vtkTransfrom, which describes linear transformations
    as a 4x4 matrix.  Internally, transformations are stored as a 4x4
    homogenous transformation matrix.  Methods are provided for manipulating
    rotation and translations.
    """

    def __init__(self):
        """
        Initialize the pose class.  It will be set to PostMultiply mode.
        """

        self.PostMultiply()
        #This is a bool flag that specifies whether this pose has been obtained
        #maunally or through optimization
        self._isOptimized = False


    def SetPose(self,pose):
        """
        Set the position and orientation of the transform.  Pose is a vector
        made up of (position[3],orientation[3])
        """
        try:
            assert len(pose)==6
        except AssertionError:
            print "Expected 'pose' to contain 6 elements, got %i" % len(pose)
            raise

        self._Position = pose[:3]
        self._Orientation = pose[3:]

        #First reset transformation to identity (otherwise rotations are concatenated)
        self.Identity()

        matrix = self.GetMatrix()
        #Set Position
        for i in range(3):
            matrix.SetElement(i,3,pose[i])

        #Set Rotations
        self.RotateX(pose[3])
        self.RotateY(pose[4])
        self.RotateZ(pose[5])


    def GetPythonArray(self):
        """
        A convienience function for returning a 4x4 matrix as a python numpy array
        """
        pyMatrix = zeros((4,4))
        matrix=self.GetMatrix()
        for i in range(4):
            for j in range(4):
                pyMatrix[i][j]=matrix.GetElement(i,j)

        return pyMatrix

    def Print(self):
        """
        To replicate functionality of JointTrack, it looks like this method
        is used to print the pose data to a text file.
        """
        translation = "%.4f,%.4f,%.4f" % (self.GetPosition())
        rotation = "%.4f,%.4f,%.4f" % (self.GetOrientation())
        return translation + "," + rotation

    def Save(self,file):
        """
        Save (cPickle.dump) data to file object.
        """

    def Load(self):
        """
        Load (cPickle.load) data from file
        """

    def PrintTableHeaders(self):
        """
        """


